import Detector from "../detector";
import GjkEpa from "./gjk-epa";
import CONSTANT from "../../constant";

export default class GjkEpaDetector extends Detector{
    constructor() {
        super(false);
    }
    detectImpl(result,geom1,geom2,tf1,tf2,cachedData) {
        let gjkEpa = GjkEpa.instance;
        let g1 = geom1;
        let g2 = geom2;
        let status = gjkEpa.computeClosestPointsImpl(g1,g2,tf1,tf2, CONSTANT.SETTING_ENABLE_GJK_CACHING ? cachedData : null,true);
        result.incremental = true;
        if(status !== CONSTANT.GJK_EPA_RESULT_STATE_SUCCEEDED) {
            console.log("GjkEpaDetector:","GJK/EPA failed: status=" + status);
            return;
        }
        if(gjkEpa.distance > g1._gjkMargin + g2._gjkMargin) {
            return;
        }
        let v = gjkEpa.closestPoint1.elements;
        let pos1X = v[0],pos1Y = v[1],pos1Z = v[2];
        let v1 = gjkEpa.closestPoint2.elements;
        let pos2X = v1[0],pos2Y = v1[1],pos2Z = v1[2];
        let normalX = pos1X - pos2X,normalY = pos1Y - pos2Y,normalZ = pos1Z - pos2Z;
        if(normalX * normalX + normalY * normalY + normalZ * normalZ === 0) {
            return;
        }
        if(gjkEpa.distance < 0) {
            normalX = -normalX;normalY = -normalY;normalZ = -normalZ;
        }
        let l = normalX * normalX + normalY * normalY + normalZ * normalZ;
        if(l > 0) {
            l = 1 / Math.sqrt(l);
        }
        normalX *= l;normalY *= l;normalZ *= l;
        this.setNormal(result,normalX,normalY,normalZ);
        pos1X += normalX * -g1._gjkMargin;pos1Y += normalY * -g1._gjkMargin;pos1Z += normalZ * -g1._gjkMargin;
        pos2X += normalX * g2._gjkMargin;pos2Y += normalY * g2._gjkMargin;pos2Z += normalZ * g2._gjkMargin;
        this.addPoint(result,pos1X,pos1Y,pos1Z,pos2X,pos2Y,pos2Z,g1._gjkMargin + g2._gjkMargin - gjkEpa.distance,0);
    }
}